# -- coding: utf-8 --
"""
# [Example] Miahx© 交互框架信息表示模型的 DDS 消息收发示例

**模块：MIAHX© / 框架基础服务层（FFST） / 交互框架信息表示模型（IFIRM）**

Version: 0.4.10,
Last updated: October 16, 2024

## Contributor(s)
* YAO Naiming <[naiming@iscas.ac.cn](mailto:naiming@iscas.ac.cn)>

## Change log
### v0.4.10
- .16: [更新] 新增 veh_annotation 字段
- .13: [更新] 适配新消息定义
- .9: [更新] 支持 bbox_*, road_* 和 veh_*
- .8: [更新] packed 模式支持 timestamps 和 sns 字段 (deprecated)
- .2: [更新] 适配新接口 (deprecated)
### v0.4.7
- .30：[更新] 调整测试数据生成逻辑
- .24：[新增] 初始实现

---
Copyright © 2024-2025 MIAHX Contributors, ISCAS, and ASIC. All Rights Reserved.
Licensed under Apache-2.0.
"""

import math
import random
import threading
import time

from miahx_support.dds.fastdds import *
from miahx_support.stag import STag

import launcher


def bng_cls_writer(p: Participant, e: threading.Event):
    def _generate(_x):
        return 1.0 / (1.0 + math.pow(math.e, -_x))

    stag = STag(**dict(sensor_channel=1, sensor_domain='CARRIER', sensor_type='VBNG_CLASSICAL',
                       sensor_channel_type='IGNORE', sensor_modality='VEC', sensor_data_format='CSV')).encode()
    gears = ['D', 'S1', 'S2', 'R', 'N', 'P']
    k = 0
    writer = BngClassicalStreamWriter(p, stag=stag, srate=10, schns=[str(i) for i in range(2)])
    while not e.is_set():
        i = random.randint(0, len(gears) - 1)
        chunk = [(100, 12.23, dict(veh_annotation='CAR', ecu_gear=gears[i], bbox_front_bottom_left=[1, 2, 3])),
                 (103, 22.23, dict(ecu_wheelspeed=50 * _generate(k), road_speed_limit=80))]
        writer.write(chunk, 0, 0)
        time.sleep(1)
        k += 0.1


def bng_cls_reader(p: Participant, e: threading.Event):
    def on_data_available(data):
        hdr = data.header()
        dm = hdr.header().domain()
        src = hdr.header().source()
        prefix = f'[{dm}::{src}::{name}]'

        schns = tuple(hdr.schns())
        samples = list(data.samples())
        log.info(f'{prefix} stag = `{hdr.stag()}`')
        log.info(f'{prefix} srate = `{hdr.srate()}`')
        log.info(f'{prefix} schns = `{schns}`')
        log.info(f'{prefix} elements = [{len(samples)}]')

        for i, sample in enumerate(samples):
            log.info(f'{prefix} Element [{i}]:')
            log.info(f'{prefix} \t\tveh_id = `{sample.veh_id()}`')
            log.info(f'{prefix} \t\tveh_name = `{sample.veh_name()}`')
            log.info(f'{prefix} \t\tveh_annotation = `{sample.veh_annotation()}`')
            log.info(f'{prefix} \t\tecu_gear = `{sample.ecu_gear()}`')
            log.info(f'{prefix} \t\tecu_wheelspeed = `{sample.ecu_wheelspeed()}`')
            log.info(f'{prefix} \t\tecu_throttle = `{sample.ecu_throttle()}`')
            log.info(f'{prefix} \t\tecu_throttle_input = `{sample.ecu_throttle_input()}`')
            log.info(f'{prefix} \t\tecu_brake = `{sample.ecu_brake()}`')
            log.info(f'{prefix} \t\tecu_brake_input = `{sample.ecu_brake_input()}`')
            log.info(f'{prefix} \t\tecu_steering = `{sample.ecu_steering()}`')
            log.info(f'{prefix} \t\tecu_steering_input = `{sample.ecu_steering_input()}`')
            log.info(f'{prefix} \t\tecu_trip = `{sample.ecu_trip()}`')
            log.info(f'{prefix} \t\tdamage_damage = `{sample.damage_damage()}`')
            log.info(f'{prefix} \t\tstate_pos = `{tuple(sample.state_pos())}`')
            log.info(f'{prefix} \t\tstate_dir = `{tuple(sample.state_dir())}`')
            log.info(f'{prefix} \t\tbbox_front_bottom_left = `{tuple(sample.bbox_front_bottom_left())}`')
            log.info(f'{prefix} \t\troad_speed_limit = `{sample.road_speed_limit()}`')
            log.info(f'{prefix} End Element [{i}] --------')

    with BngClassicalStreamReader(p, on_data_listeners=[on_data_available]) as reader:
        name = type(reader).__name__
        log.success(f'[{name}] Topic: {reader.topic_name}')
        reader.wait(e)


if __name__ == '__main__':
    log = launcher.log
    event = threading.Event()
    participant = Participant()
    packed_data = True

    launcher.run(lambda: (event, [
        threading.Thread(target=bng_cls_writer, args=(participant, event), daemon=False),
        threading.Thread(target=bng_cls_reader, args=(participant, event), daemon=False),
    ]))
